#ifndef MATRIX33_H
#define MATRIX33_H

#include "typedefs.h"
#include "vector3.h"
#include <Eigen/Core>

namespace Math{

class Matrix33
{
#ifdef USE_FLOAT
    Eigen::Matrix3f v;
#else
    Eigen::Matrix3d v;
#endif
public:
    Matrix33();

    Matrix33(const Matrix33 &a) { v = a.v;}

#ifdef USE_FLOAT
    Matrix33(const Eigen::Matrix3f &ev) { v = ev; }
#else
    Matrix33(const Eigen::Matrix3d &ev) { v = ev; }
#endif

    Matrix33(bool zero) { if(zero) v.setZero(); else v.setIdentity(); }

    Matrix33(const Vector3 &c1,const Vector3 &c2,const Vector3 &c3)
    {
       v.col(0) = c1.getEigen();
       v.col(1) = c2.getEigen();
       v.col(2) = c3.getEigen();
    }

    void set(const Real &a1,const Real &a2,const Real &a3,
             const Real &a4,const Real &a5,const Real &a6,
             const Real &a7,const Real &a8,const Real &a9)
    {
        (*this)(0,0) = a1; (*this)(0,1) = a2; (*this)(0,2) = a3;
        (*this)(1,0) = a4; (*this)(1,1) = a5; (*this)(1,2) = a6;
        (*this)(2,0) = a7; (*this)(2,1) = a8; (*this)(2,2) = a9;
    }

    void setValue(const Real &val) { v.setConstant(val); };

    static Matrix33 IDENTITY;
    static Matrix33 ZERO;

    Real& operator()(const int &r, const int &c) { return v(r,c); }

    void setDiagonal(const Vector3 &val) { v.diagonal() = val.getEigen(); }

    Vector3 operator*(const Vector3 &val) const { return Vector3(v*val.getEigen()); }
};

}
#endif // MATRIX33_H
